//
// Created by k on 25-6-26.
//
#include "unitree_b2.h"
#define TOPIC_HIGHSTATE "rt/sportmodestate"
const int MOTION_MODE = stand_down;
using namespace unitree::common;
using namespace unitree::robot;

#define TOPIC_LOWCMD "rt/lowcmd"
#define TOPIC_LOWSTATE "rt/lowstate"
// 电池
// IMU
// odom
// state
// vel
//video
UnitreeB2::UnitreeB2(std::string network_interface) {
    this->network_interface_ = network_interface;
    unitree::robot::ChannelFactory::Instance()->Init(0, network_interface_);
    sport_client_.SetTimeout(5.0f);
    sport_client_.Init();
    robot_state_client_.SetTimeout(5.0f);
    robot_state_client_.Init();

    std::string service_name = "ai_sport";
    int status = 0;
    int ret = robot_state_client_.ServiceSwitch(service_name, 1, status);

    sport_mode_state_suber_.reset(new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>(TOPIC_HIGHSTATE));
    sport_mode_state_suber_->InitChannel(std::bind(&UnitreeB2::HighStateHandler, this, std::placeholders::_1), 1);

    /*create subscriber*/
    low_state_suber_.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
    low_state_suber_->InitChannel(std::bind(&UnitreeB2::LowStateMessageHandler, this, std::placeholders::_1), 1);
}

void UnitreeB2::LowStateMessageHandler(const void* message)
{
    low_state_ = *(unitree_go::msg::dds_::LowState_*)message;
    data_.voltage = low_state_.bms_state().soc();
    data_.current = low_state_.bms_state().current();
    std::cout << "voltage:" <<data_.voltage << std::endl;
    std::cout << "current:" <<data_.current << std::endl;
}
void UnitreeB2::HighStateHandler(const void *message)
{
    sport_mode_state_ = *(unitree_go::msg::dds_::SportModeState_ *) message;
    // std::cout << "Position: " << state.position()[0] << ", " << state.position()[1] << ", " << state.position()[2] << std::endl;
    data_.odom_x = sport_mode_state_.position()[0];
    data_.odom_y = sport_mode_state_.position()[1];
    data_.odom_z = sport_mode_state_.imu_state().rpy()[2];

    data_.vel_x =  sport_mode_state_.velocity()[0];
    data_.vel_y =  sport_mode_state_.velocity()[1];
    data_.vel_z =  sport_mode_state_.yaw_speed();

    std::cout << "vel_x:" <<data_.vel_x << std::endl;
    std::cout << "vel_y:" <<data_.vel_y << std::endl;
    std::cout << "vel_z:" <<data_.vel_z << std::endl;
    std::cout << "odom_x:" <<data_.odom_x << std::endl;
    std::cout << "odom_y:" <<data_.odom_y << std::endl;
    std::cout << "odom_z:" <<data_.odom_z << std::endl;

    // std::cout << "IMU rpy: " << state.imu_state().rpy()[0] << ", " << state.imu_state().rpy()[1] << ", " << state.imu_state().rpy()[2] << std::endl;
};
void UnitreeB2::RobotControl() {

    current_time_ += dt_;
    double px_local, py_local, yaw_local;
    double vx_local, vy_local, vyaw_local;
    double px_err, py_err, yaw_err;
    double time_seg, time_temp;

    unitree::robot::go2::PathPoint path_point_tmp;
    std::vector<unitree::robot::go2::PathPoint> path;

    switch (MOTION_MODE)
    {
        case normal_stand:            // 0. idle, default stand
            // sport_client.SwitchGait(0); // 0:idle; 1:tort; 2:tort running; 3:climb stair; 4:tort obstacle
            sport_client_.StandUp();
            break;

        case balance_stand:                  // 1. Balance stand (controlled by dBodyHeight + rpy)
            // sport_client.Euler(0.1, 0.2, 0.3); // roll, pitch, yaw
            // sport_client.BodyHeight(0.0);      // relative height [-0.18~0.03]
            sport_client_.BalanceStand();
            break;

        case velocity_move: // 2. target velocity walking (controlled by velocity + yawSpeed)
            sport_client_.Move(0.3, 0, 0.3);
            break;

        case stand_down: // 4. position stand down.
            sport_client_.StandDown();
            break;

        case stand_up: // 5. position stand up
            sport_client_.StandUp();
            break;

        case damp: // 6. damping mode
            sport_client_.Damp();
            break;

        case recovery_stand: // 7. recovery stand
            sport_client_.RecoveryStand();
            break;

        case sit:
            if (flag == 0)
            {
                // sport_client_.Sit();
                flag = 1;
            }
            break;

        case rise_sit:
            if (flag == 0)
            {
                // sport_client_.RiseSit();
                flag = 1;
            }
            break;

        case stop_move: // stop move
            sport_client_.StopMove();
            break;

        default:
            sport_client_.StopMove();
    }
}

int test_unitree_main(int argc, char *argv[]) {
    UnitreeB2 b2("test");
    while (true) {}
}



